[CENIC] Support distance constraints - #24776

@joemasterjohn made 1 comment.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

a discussion (no related file):
Blocking work on this until #24769 merges.

+a:@sherm1 for feature review, please.

This one still depends on the refactor in #24769, so everything except the last commit can be ignored. No rush on this, I just wanted to get your eyes on it as soon as I could. This is the last in the saga of holonomic constraint support.

@joemasterjohn made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

@joemasterjohn resolved 1 discussion.
Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, labeled "do not merge" (waiting on sherm1).

Feature with one issue regarding handling of a bad initial condition, PTAL.

@sherm1 reviewed 30 files and all commit messages, and made 10 comments.
Reviewable status: 9 unresolved discussions, needs at least two assigned reviewers, labeled "do not merge" (waiting on joemasterjohn).

multibody/contact_solvers/icf/distance_constraints_pool.h line 30 at r2 (raw file):

this is a compliant (spring-damper) constraint: with p̂ the unit vector from P
to Q, stiffness k and damping c, the scalar impulse is
  γ = −k⋅(d − ℓ) − c⋅ḋ ∈ ℝ

BTW this looks like a force to me -- shouldn't there be a δt in there somewhere to make it an impulse?

multibody/contact_solvers/icf/distance_constraints_pool.cc line 29 at r2 (raw file):

template <typename T>
Vector1<T> DistanceConstraintsPool<T>::CalcConstraintVelocity(
    int k, const Vector6<T>& V_WB, const Vector6<T>* V_WA) const {

BTW is there a reason these have to be Vector6's rather than SpatialVelocities? The latter would be more "Drake like" and shorten some of the code below (I think).

multibody/contact_solvers/icf/distance_constraints_pool.cc line 30 at r2 (raw file):

Vector1<T> DistanceConstraintsPool<T>::CalcConstraintVelocity(
    int k, const Vector6<T>& V_WB, const Vector6<T>* V_WA) const {
  // vc = ḋ = p̂ᵀ⋅(v_W_Bq − v_W_Ap), the rate of change of distance.

minor: to check this I had to imagine what d might be -- it didn't seem to be defined anywhere. This makes sense with d = ∥p_WBq − p_WAp∥₂. That should be said somewhere (even here would be ok).

nit: the monogram notation for the time derivative of p_WBq is v_WBq without the extra underscore.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 34 at r2 (raw file):

  const Vector3<T>& w_WB = V_WB.template head<3>();
  const Vector3<T>& v_WBo = V_WB.template tail<3>();
  const Vector3<T> v_W_Bq = v_WBo + w_WB.cross(p_BQ_W_[k]);

nit: v_W_Bq -> v_WBq

multibody/contact_solvers/icf/distance_constraints_pool.cc line 53 at r2 (raw file):

  // at Q on B and −γ⋅p̂ at P on A. Shift each to the body origin.
  const Vector3<T>& p_hat_W = p_hat_W_[k];
  const Vector3<T> f_B = gamma(0) * p_hat_W;

BTW Consider using a different symbol for an impulse to distinguish it from a force. Maybe j_B?

multibody/contact_solvers/icf/icf_builder.cc line 672 at r2 (raw file):

          "between bodies '{}' and '{}' is {}, which is nonphysically small "
          "compared to the constraint's free length, {}.",
          body_A.name(), body_B.name(), ExtractDoubleOrThrow(d0), length));

Can you say more about why this error condition? I would have expected that as long as ℓ is a reasonable non-zero length then it doesn't matter what the current value of d is -- we should be able to assemble the distance constraint as we can for a weld or ball that are initially unsatisfied. This seems to be setting some requirement on the initial unassembled model specification. Is that also the case in SAP, and is this documented somewhere?

I can see why you might want to require d0 to be non-zero to facilitate normalization of p_PQ_W below, but for that purpose almost anything non-zero would work, even 1e-15. You could even allow d0==0 if you provide an arbitrary p_hat just to help get the distance constraint assembled.

Also, I don't see a unit test for this condition. If possible, I'd suggest removing the restriction on d0 altogether to avoid annoying failures when resolving initial conditions. There ought to be an error condition for ℓ too small though if there isn't already. You would still need a unit test for the d0=0 initial condition since there would be a special case using an arbitrary p_hat for the first step.

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 150 at r2 (raw file):

  EXPECT_NEAR(distance, expected_distance, 2e-3);
}

BTW should have an initial conditions test where the points P and Q are initially coincident (d0=0) to see that we can escape from that trap.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

}

/* Checks that pool.ReduceInto does not incur any heap allocations on a

nit: space after period missing

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 85 at r2 (raw file):

/* Verifies that distance constraints produce correct data. Uses a rigid and a
compliant constraint (see AddDistanceConstraints), exercising both R branches.

BTW not sure what "R branches" means?

@joemasterjohn made 7 comments and resolved 7 discussions.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

multibody/contact_solvers/icf/distance_constraints_pool.h line 30 at r2 (raw file):

Good catch. Added here and in the spot I copied it from.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 29 at r2 (raw file):

I don't have a compelling reason. Vector6 has just been the convention cenic/icf has used since the beginning.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 30 at r2 (raw file):

Done. d was introduced in the Class docs, but I'll add it here for clarity.

multibody/contact_solvers/icf/icf_builder.cc line 672 at r2 (raw file):

This was also a remnant from the SAP constraint. But after f2f we decided that this isn't strictly an error condition. I've refactored to be able to handle the coincident initial condition and added test coverage, PTAL.

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 150 at r2 (raw file):

Done. Covered by the new test in icf_builder_test.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

Cargo culted from the other constraints. This is talking about the method pool.ReduceInto().

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 85 at r2 (raw file):

"R" is referring to the regularization; I added more comments to clarify. The base class chooses between the near-rigid regularization and that computed by the user provided stiffness (whichever is softer). That's the branching this is referring to.

Thanks Joe. All good, one suggestion.
+a:@jwnimmer-tri for Thu platform review per rotation, please

@sherm1 reviewed 6 files and all commit messages, made 3 comments, and resolved 2 discussions.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn and jwnimmer-tri).

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

Oh, sorry -- I misread that!

multibody/contact_solvers/icf/test/icf_builder_test.cc line 466 at r3 (raw file):

  const auto& pool = model.distance_constraints_pool();
  EXPECT_NEAR(pool.p_hat_W()[0].norm(), 1.0, 1e-14);
  EXPECT_NEAR(pool.g0()[0](0), -kFreeLength, 1e-14);

BTW consider adding a short simulation to verify that it actually ends up near the free length

Checkpoint about halfway through. I'll need to circle back on the rest later today.

@jwnimmer-tri reviewed 27 files and all commit messages, and made 8 comments.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

multibody/contact_solvers/icf/icf_builder.cc line 618 at r3 (raw file):

  int index = 0;
  for (const auto& [id, params] : params_map) {

nit We can make it more clear that id is unused.

Suggestion:

for (const auto& [_, params] : params_map) {

multibody/contact_solvers/icf/icf_builder.cc line 631 at r3 (raw file):

    //  happen is in MbP::Finalize() after the topology has been finalized.
    if (A_anchored && B_anchored) {
      const std::string msg = fmt::format(

nit msg is not a form of abbreviation allowed by our style guide.

(The easiest solution here is to put the fmt::format directly into the logic_error constructor; naming it doesn't help with clarity at all.)

multibody/contact_solvers/icf/icf_builder.cc line 664 at r3 (raw file):

    // When P and Q are (nearly) coincident, p̂ is undefined. The free length ℓ
    // is strictly positive (enforced by DistanceConstraintParams), so there

nit Something here is grammatically incorrect near "there".

multibody/contact_solvers/icf/icf_builder.cc line 671 at r3 (raw file):

    // is purely a divide-by-zero guard for the normalization below.
    constexpr double kMinimumDistance = 1.0e-14;
    const Vector3<T> p_hat_W = ExtractDoubleOrThrow(d0) < kMinimumDistance

nit Is the "extract double" really necessary? AutoDiff should already have a comparison operator that does this implicitly:

/** Standard comparison operator. Discards the derivatives. */
inline bool operator<(const AutoDiff& a, double b) {
  return a.value() < b;
}

Assuming this gets removed, also remove the #include statement for it.

Code quote:

ExtractDoubleOrThrow(d0) < kMinimumDistance

multibody/contact_solvers/icf/icf_data.h line 122 at r3 (raw file):

  @param patch_sizes Number of contact pairs for each patch constraint, of size
                     equal to the number of patches. */
  // TODO(sherm1) This argument list will get out of hand as we add more

BTW We're at the point of being out-of-hand. Either Joe or Sherm should tackle this in a follow-up PR.

It's aggravated by the fact that here (only) we've sorted the span<const int> arguments to the end of the list, whereas everywhere else we list all constraints in a row they are sorted differently (e.g., ball, coupler, distance, gain, limit, patch, weld -- in that order).

multibody/contact_solvers/icf/test/icf_builder_test.cc line 387 at r3 (raw file):

}

GTEST_TEST(IcfBuilder, DistanceConstraint) {

At least from the comments describing the new tests, I can't convince myself that the "swap bodies A and B in case B is anchored" logic in the builder is exercised.

multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc line 440 at r3 (raw file):

     &AddCouplerConstraint<T>, &AddDistanceConstraints<T>,
     &AddGainConstraints<T>, &AddLimitConstraints<T>, &AddPatchConstraints<T>,
     &AddWeldConstraints<T>));

nit The order of instantiations here seems inconsistent. It is neither alphabetical nor does it match the order of functions in the h/cc file.

platform.

@jwnimmer-tri reviewed 4 files and made 9 comments.
Reviewable status: 16 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

           const T& stiffness, const T& damping);

  /* Hooks required by HolonomicConstraintsPool (CRTP). */

nit In the HolnomicConstraintsPool specification for CalcSpatialImpulses and CalcHessianBlocks, it is not made clear whether the non-const pointer arguments are [out] or [in,out].

multibody/contact_solvers/icf/distance_constraints_pool.h line 94 at r3 (raw file):

  EigenPool<Vector3<T>> p_AP_W_;   // Position of P in A, expressed in W.
  EigenPool<Vector3<T>> p_BQ_W_;   // Position of Q in B, expressed in W.
  EigenPool<Vector3<T>> p_hat_W_;  // Unit vector P→Q, expressed in W.

BTW These comments seem a bit redundant, both with monogram itself and with the constructor documentation.

Code quote:

  EigenPool<Vector3<T>> p_AP_W_;   // Position of P in A, expressed in W.
  EigenPool<Vector3<T>> p_BQ_W_;   // Position of Q in B, expressed in W.
  EigenPool<Vector3<T>> p_hat_W_;  // Unit vector P→Q, expressed in W.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 23 at r3 (raw file):

  p_BQ_W_[index] = p_BQ_W;
  p_hat_W_[index] = p_hat_W;
  // Constraint function g₀ = d₀ − ℓ ∈ ℝ.

BTW I'm not sure copy-pasting the comment from the header file documentation of this method into this spot is really adding any useful information.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 89 at r3 (raw file):

    G_Ap->template bottomRightCorner<3, 3>() = Gt;

    // G_cross = −Φ(p_BQ)ᵀ⋅G⋅Φ(p_AP).

The specification of CalcHessianBlocks does not promise that G_Ap and G_cross are either both null or both non-null, yet we seem to be relying on that here.

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 148 at r3 (raw file):

  const double distance = SimulateAndGetFinalDistance(kStiffness, kDamping);
  const double expected_distance = kFreeLength + kMass * kGravity / kStiffness;
  EXPECT_NEAR(distance, expected_distance, 2e-3);

nit The 2 here seems a bit magical. Locally for me, 1mm passes. If allowing the extra factor of 2 is important, we should comment why.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 98 at r3 (raw file):

  model.ResizeData(&data);
  EXPECT_EQ(data.num_velocities(), model.num_velocities());
  EXPECT_EQ(data.distance_constraints_data().num_constraints(), 0);

BTW This feels weird to be here, when the very next sentence is "should be no distance constraints". Seems like this should be part of the next stanza.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 192 at r3 (raw file):

dynamic with 6 DOFs each. */
template <typename T>
void MakeModelForDistance(IcfModel<T>* model, double time_step = 0.01) {

nit The time_step argument is never varied by any callers, so it shouldn't be an argument.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 207 at r3 (raw file):

  MatrixX<T>& M0 = params->M0;
  M0 = MatrixX<T>::Identity(nv, nv);

BTW Do we need identity here? It seems like zero would be equally good, and possibly clearer?

@joemasterjohn made 4 comments and resolved 9 discussions.
Reviewable status: 7 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri and sherm1).

multibody/contact_solvers/icf/icf_builder.cc line 664 at r3 (raw file):

Thanks. I think I just left a rogue "there" there.

multibody/contact_solvers/icf/icf_builder.cc line 671 at r3 (raw file):

Done.

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 148 at r3 (raw file):

It's not important. I originally set CENIC's accuracy too loose, but with an accuracy of 1e-3 all of these tolerances can be 1mm.

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 207 at r3 (raw file):

Nope, it all gets overridden, so zero works.

@jwnimmer-tri reviewed 4 files and all commit messages.
Reviewable status: 7 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

@joemasterjohn made 6 comments and resolved 4 discussions.
Reviewable status: 3 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

Done. I've added full documentation to the declarations in DistanceConstraintsPool and to the class documentation in HolonomicConstraintsPool.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 23 at r3 (raw file):

Removed.

multibody/contact_solvers/icf/distance_constraints_pool.cc line 89 at r3 (raw file):

Done. I fleshed out the documentation in DistanceConstraintsPool and HolonomicConstraintsPool. PTAL.

multibody/contact_solvers/icf/test/icf_builder_test.cc line 387 at r3 (raw file):

Done. Added DistanceConstraintAnchoredBodyB to exercise this.

multibody/contact_solvers/icf/test/icf_builder_test.cc line 466 at r3 (raw file):

Done — added RigidCoincidentInitialCondition to the sim test.

multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc line 440 at r3 (raw file):

Done. Reordered the instantiation list to match the order the functions are defined in the .cc.

@jwnimmer-tri reviewed 4 files and all commit messages, made 1 comment, and resolved 2 discussions.
Reviewable status: 2 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn and sherm1).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

@sherm1 PTAL at the updated approach to CRTP / C++ Concepts.

The distance constraints pool is now different than all other holonomic constraints. (They don't document each individual hook.)

I'm not sure that the obvious solution of copy-pasting this distance pool docs onto all of the other constraints would be a benefit, though.

@joemasterjohn made 1 comment and resolved 1 discussion.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri and sherm1).

multibody/contact_solvers/icf/icf_data.h line 122 at r3 (raw file):

Done. See #24822.

@sherm1 reviewed 8 files and all commit messages, and made 1 comment.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

I love the new documentation! My suggestion would be to move it verbatim to the hooks documentation in holonomics_constraint_pool.h, replacing the less-detailed documentation there. Then add a comment here and in the other holonomic constraints like See holonomics_constraint_pool.h for the requirements for each of these required hooks.`

An alternative would be to move it to the requires clause where the same methods are listed. But either way I think it should be centralized rather than duplicated since that risks out-of-sync comments in the future.

@jwnimmer-tri made 1 comment.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

Another random idea. Can we declare the "virtual" methods in the base class, marking them as = delete;? That would allow normal Doxygen-like commenting in a familiar way.

@joemasterjohn made 1 comment.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri and sherm1).

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

Ah that's a great idea, actually. I was already doing = delete; for FlipNegatesG0(), so I should have just thought of that. Ok, now all of the documentation is consolidated to the base class in Doxygen style.

@jwnimmer-tri reviewed 3 files and all commit messages, made 1 comment, and resolved 1 discussion.
Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

multibody/contact_solvers/icf/holonomic_constraints_pool.h line 272 at r6 (raw file):

             Vector6<double>* spatial_impulse, Matrix6<double>* hessian_block,
             int k, bool flip) {
      /* Computes vc ∈ ℝᴺ, the constraint velocity of constraint k. V_WA will be

BTW For my taste, I would (at least) delete all of the comments in this section that describe the semantics of the methods. The comments are duplicative (brittle) w.r.t the API docs on the deleted signatures.

In fact, for my taste I would get rid of the concept entirely. GSG says not to define new concepts, and I don't think it actually improves error messages enough to be worth the effort. If I remove the static_assert about the concept, and introduce a mistake where a method is absent in the derived class, I get a fine compilation error already.

But I'll defer to you and Sherm.

@joemasterjohn made 1 comment and resolved 1 discussion.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),sherm1(platform) (waiting on jwnimmer-tri and sherm1).

multibody/contact_solvers/icf/holonomic_constraints_pool.h line 272 at r6 (raw file):

True. I don't think the concept is adding anything worth the bitrot. Removed.